/**
 * @file VisualMonitor.cpp
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 视觉监视类
 * @date 2021-09-15
 *
 */
#include "VisualMonitor.h"

/**
 * @brief 视觉识别器初始化
 */
Visual::Visual() {
    trackers.reserve(10);//trackers无需下标访问，亦不通过数量限制大小
    appear_time = 0;
    this->detector = nullptr;
}

AutoAim::AutoAim() {
    //predictor里面储存了时间,陀螺仪序列，不能通过构造函数每帧更新
    predictor = make_unique<ArmorPredict>();
    compensator = make_unique<Compensate>();
    //决策器初始化
    judger= make_unique<ArmorJudge>();
}

BuffActivate::BuffActivate() {
    predictor = make_unique<RunePredict>();
    compensator = make_unique<Compensate>();
    judger= make_unique<RuneJudge>();
}

/**
 * @brief 更新当前帧信息
 * @param src 源图
 * @param gyroData 陀螺仪数据
 * @return 发给电控的数据
 */
Point2f AutoAim::updateFrame(Mat &src, GyroData &gyroData) {
    //指针指向地址以便后续使用
    this->gyroData= make_shared<GyroData>(gyroData);
    //图像识别检测
    detector = std::make_shared<ArmorDetect>(src);
    //记录时间戳
    appear_time = clock();
    //预测初始化
    predictor->updateSequence(gyroData, appear_time);
    //更新追踪序列
    this->updateTrackers();
    //绘制预测点,倘若没有装甲板构成的序列，则不会绘制任何图案
    if (!detector->getCurrentFeatures().empty())
        for (const auto &tracker: trackers) {
            //只绘制当前识别到的
            if (!tracker->isShine())
                detector->draw(tracker->getPredictedCenter());
        }
    //返回总预测偏角
    return total_move;
}

/**
 * @brief 获取pnp距离最近的序列
 * @return 该序列指针
 */
tracker_ptr Visual::getClosedTracker() {
    //min_element返回迭代器，*后才是trackerA_ptr
    return *std::min_element(this->trackers.begin(), this->trackers.end(),
                             [&](const tracker_ptr &tracker_1, const tracker_ptr &tracker_2) {
                                 return tracker_1->getPnp().distance < tracker_2->getPnp().distance;
                             });
}

/**
 * @brief 装甲板序列匹配
 */
void AutoAim::updateTrackers() {
    if (!this->detector->isTrackerMatched) {
        // 如果 trackers 为空,先为每个识别到的armor开辟序列
        if (this->trackers.empty()) {
            for (auto armor: this->detector->getCurrentFeatures()) {
                this->trackers.emplace_back(new ArmorTracker(armor));
            }
            total_move = this->updateInfo();
            detector->isTrackerMatched = true;
            return;
        }
        // 如果当前帧识别到的装甲板数量 > 序列数量
        if (this->detector->getCurrentFeatures().size() > this->trackers.size()) {
            for (const auto &tracker: this->trackers) {
                // 离 tracker 最近的 armor 及其距离
                Features_ptr min_dis_armor = nullptr;
                float min_dis = MAXFLOAT;
                for (const auto &armor: this->detector->getCurrentFeatures()) {
                    if (!armor->getMatchMessage()) {
                        float distance = getDistance(armor->getCenter(), tracker->getGoalFeature()->getCenter());
                        // 更新最小值
                        if (distance < min_dis) {
                            min_dis_armor = armor;
                            min_dis = distance;
                        }
                    }
                }
                tracker->update(min_dis_armor);
                min_dis_armor->setMatchMessage(true);
            }
            // 没有匹配到的装甲板作为新的序列
            for (auto armor: this->detector->getCurrentFeatures()) {
                if (!armor->getMatchMessage()) {
                    this->trackers.emplace_back(new ArmorTracker(armor));
                }
            }
        }
            // 如果当前帧识别到的装甲板数量 < 序列数量
        else if (this->detector->getCurrentFeatures().size() < this->trackers.size()) {
            for (const auto &armor: this->detector->getCurrentFeatures()) {
                // 离 armor 最近的 tracker 及其距离
                tracker_ptr min_dis_tracker = nullptr;
                float min_dis = MAXFLOAT;
                for (const auto &tracker: this->trackers) {
                    if (!tracker->getMatchMessage()) {
                        float distance = getDistance(tracker->getGoalFeature()->getCenter(), armor->getCenter());
                        // 更新最小值
                        if (distance < min_dis) {
                            min_dis_tracker = tracker;
                            min_dis = distance;
                        }
                    }
                }
                min_dis_tracker->update(armor);
                min_dis_tracker->setMatchMessage(true);
            }
            // 循环遍历 trackers
            for (const auto &tracker: this->trackers) {
                // 没有匹配到的序列传入 nullptr
                if (!tracker->getMatchMessage()) {
                    tracker->update(nullptr);
                } else {
                    // 匹配到的 tracker 标志重置为 false 供下一帧循环使用
                    tracker->setMatchMessage(false);
                }
            }
        }
            // 如果当前帧识别到的装甲板数量 = 序列数量
        else {
            for (const auto &tracker: this->trackers) {
                // 离 tracker 最近的 armor 及其距离
                Features_ptr min_dis_armor = nullptr;
                float min_dis = MAXFLOAT;
                for (const auto &armor: this->detector->getCurrentFeatures()) {
                    if (!armor->getMatchMessage()) {
                        float distance = getDistance(armor->getCenter(), tracker->getGoalFeature()->getCenter());
                        if (distance < min_dis) {
                            min_dis_armor = armor;
                            min_dis = distance;
                        }
                    }
                }
                /**
                 * @brief 如果装甲板在一帧内位置变化特别大 (大于原来距离的 MAX_DELTA_DIS_RATIO 倍) ,
                 *        且速度发生较大突变，则创建新序列
                 */
                if (tracker->isSuddenChange(min_dis_armor, min_dis)) {
                    //创建新序列
                    this->trackers.emplace_back(new ArmorTracker(min_dis_armor));
                    //原来的序列打入 nullptr
                    tracker->update(nullptr);
                }
                    // 若变化不大, 正常更新
                else {
                    tracker->update(min_dis_armor);
                }
            }
        }
        total_move = this->updateInfo();
        detector->isTrackerMatched = true;
    }
}

/**
 * @brief 更新序列匹配完后的信息
 * @return 最终预测angle
 */
cv::Point2f AutoAim::updateInfo() {
    for (auto it = trackers.begin(); it != trackers.end(); it++) {
        if ((*it)->isVanished(this->gyroData->RegionMode)) {
            trackers.erase(it);//删除已丢失目标的tracker
            it--;//erase后it指向下一个目标,直接it++会跳过该目标
        } else {
            //对每一个仍在跟踪的装甲板做预测,补偿
            this->predictor->predict(*it, this->compensator);
        }
    }
    judger->Judging(trackers,detector->getCurrentFeatures().empty());
    cv::Point2f total_move(0.f, 0.f);
    //仅在当前追踪序列不为空时才更新发送数据
    if (!trackers.empty()&&judger->getTrackingTarget()!= nullptr)
/*        total_move = compensator->getXY_compensate() +
                     Point2f(getClosedTracker()->getPredictedAngle().x, getClosedTracker()->getPredictedAngle().y);*/
        total_move=judger->getTrackingTarget()->getPredictedAngle();
//        total_move = getClosedTracker()->getPredictedAngle();
    return total_move;
}

/**
 * @brief 更新当前帧信息
 * @param src 源图
 * @param gyroData 陀螺仪数据
 * @return 发给电控的数据
 */
Point2f BuffActivate::updateFrame(Mat &src, GyroData &gyroData) {
    //指针指向地址以便后续使用
    this->gyroData= make_shared<GyroData>(gyroData);
    //图像识别检测
    detector = std::make_shared<RuneDetect>(src);
    //记录时间戳
    appear_time = clock();
    //预测初始化
    predictor->updateSequence(gyroData, appear_time);
    //更新追踪序列
    this->updateTrackers();
    //绘制预测点,为使tracker和detector解耦，draw不得不外置
    if (!detector->getCurrentFeatures().empty())
        for (const auto &track: trackers) {
            //只绘制当前识别到的,防止留下过去的静止点
            if (!track->isShine())
                detector->draw(track->getPredictedCenter());
        }
    //返回总预测偏角
    return this->total_move;
}

/**
 * @brief 神符时间序列匹配
 */
void BuffActivate::updateTrackers() {
    // 若 _pRune 不为空
    if (!detector->getCurrentFeatures().empty() && !this->detector->isTrackerMatched) {
        // 容器为空则直接初始化
        if (this->trackers.empty()) {
            this->trackers.emplace_back(new RuneTracker(detector->getCurrentFeatures().back()));
            return;
        }
            // trackers = 1
        else if (this->trackers.size() == 1) {
            float delta_dis = 0.f;//仅为子类虚函数提供参数，暂并无实际意义
            if (this->trackers.back()->isSuddenChange(detector->getCurrentFeatures().back(), delta_dis)) {
                this->trackers.back()->update(nullptr);
                this->trackers.emplace_back(new RuneTracker(detector->getCurrentFeatures().back()));
            } else {
                this->trackers.back()->update(detector->getCurrentFeatures().back());
            }
        }
            // trackers > 1
        else {
            // 选择最近的装甲板
            float min_delta_angle = MAXFLOAT;
            tracker_ptr min_tracker = nullptr;
            for (const auto &tracker: this->trackers) {
                // 获取角度差
                float delta_angle = getDeltaAngle(detector->getCurrentFeatures().back()->getAngle(),
                                                  tracker->getGoalFeature()->getAngle());
                // 更新最小值
                if (delta_angle < min_delta_angle) {
                    min_delta_angle = delta_angle;
                    min_tracker = tracker;
                }
            }
            for (const auto &tracker: this->trackers) {
                if (min_tracker == tracker) {
                    tracker->update(detector->getCurrentFeatures().back());
                } else {
                    tracker->update(nullptr);
                }
            }
        }
    } else /*当前帧未识别到神符的情况*/{
        if (this->trackers.empty()) {
            return;
        } else {
            for (const auto &tracker: this->trackers) {
                tracker->update(nullptr);
            }
        }
    }
    total_move = this->updateInfo();
    detector->isTrackerMatched = true;
}

/**
 * @brief 更新序列匹配完后的信息
 * @return 最终预测angle
 */
cv::Point2f BuffActivate::updateInfo() {
    for (auto it = trackers.begin(); it != trackers.end(); it++) {
        if ((*it)->isVanished(this->gyroData->RegionMode)) {
            trackers.erase(it);//删除已丢失目标的tracker
            it--;//erase后it指向下一个目标,直接it++会跳过该目标
        } else {//只为正在跟踪的tracker更新kalman信息,当前帧未识别到的神符均不做卡尔曼和pnp更新
            if (!(*it)->isShine()) {
                predictor->predict(*it, this->compensator);
            }
        }
    }
//    judger->Judging(trackers,detector->getCurrentFeatures().empty());
    cv::Point2f total_move(0.f, 0.f);
    //仅在当前追踪序列不为空时才更新发送数据
    if (!trackers.empty())
        total_move = compensator->getXY_compensate() +
                     Point2f(getClosedTracker()->getPredictedAngle().x, getClosedTracker()->getPredictedAngle().y);
    return total_move;
}